//
// Created by 74354 on 2022/1/4.
//

#include "Robot.h"
#include <QDebug>

/* @brief append joint at end of joints
 *
 */
void Robot::appendJoint(JointType_t type,double a,double alpha,double d,double theta){
    Joint* joint = new Joint(a,alpha,d,theta);
    if(type == Rotate)
    {
        joint->type = type;
    }

    Joints.push_back(joint);
    mat j = {a,alpha,d,theta};
    DH_Mat = join_vert(DH_Mat,j);

    T_froward = T_froward * joint->T;
}

/* @brief create Joint Type for the pos
 * @return
 * @author
 */
JointPos createJointValue(mat pos,int count)
{
   JointPos jValue;
   for(int i = 0;i < count;i++)
   {
       jValue.push_back(pos(i,0));
   }
   return jValue;
}

/*
* @brief append joint at end of joints
* @param class Joint that contains a、alpha、d、theta、T_transfer
* @return void
*/
void Robot::appendJoint(Joint* joint)
{
    Joints.push_back(joint);
    mat j = {joint->a,joint->alpha,joint->d,joint->theta};
    DH_Mat = join_vert(DH_Mat,j);
    T_froward = T_froward * joint->T;
}

/* @brief remove all the Joints of robot
 *
 */
void Robot::removeAllJoints()
{
    Joints.clear();
    DH_Mat.clear();
    T_froward = mat(4,4,fill::eye);
}

/* @brief forward kinematics
 *        which would caculate the transferation with joints matrix
 *
 */
mat Robot::fkine(mat thetaMat)
{
    if(!checkTheta(thetaMat))
    {
        return T_froward;
    }
    if(thetaMat.n_rows == DH_Mat.n_rows && thetaMat.n_cols == 1)
    {
        DH_Mat(span(),3) = thetaMat;
        T_froward = eye(4,4);
        int i = 0;
        for(vector<Joint*>::iterator v = Joints.begin();v != Joints.end();++v)
        {
            (*v)->set_theta(thetaMat(i,0));
            T_froward = T_froward * (*v)->T;
            i++;
        }
    }
    return T_froward;
}

/* @brief caculate the jacobian matrix (6X6) with DH_Mat
 *       which would
 *
 */
mat Robot::jacobian() {
    int len = Joints.size();
    mat jaco(6,len,fill::eye);
    mat jaco_n(6,1,fill::zeros);
    vector<Joint*>::reverse_iterator  i = Joints.rbegin();
    mat Tn(4,4,fill::eye);
    for(;i!=Joints.rend();i++)
    {
        Tn = (*i)->T * Tn;
        if((*i)->type == Rotate) {
            jaco_n(span(0, 2), 0) = {
            {Tn(0, 3) * Tn(1, 0) - Tn(0, 0) * Tn(1, 3)},
            {Tn(0, 3) * Tn(1, 1) - Tn(0, 1) * Tn(1, 3)},
            {Tn(0, 3) * Tn(1, 2) - Tn(0, 2) * Tn(1, 3)}
        };
            jaco_n(span(3,5),0) = Tn(2,span(0,2)).t();
        }
        else if((*i)->type == Line)
        {
            jaco_n(span(0, 2), 0) = Tn(2,span(0,2)).t();
            jaco_n(span(3,5),0) = {{0},{0},{0}};
        }
        jaco(span(),--len) = jaco_n;
    }
    return  jaco;
}

/* @brief caculate the angle that in joint space with the Matrix described by ground coordinate
 *        And which would change the DH_Mat and
 */
mat Robot::ikine(mat posMat) {
    mat theta(Joints.size(),1);
    double precision = 1e-10;

    mat d_joint(Joints.size(),1,fill::zeros); // caculate the differential move of Joint
    mat JtJ,left_inv_jac,e,jaco,dq;
    mat thetaMat = DH_Mat(span(),3);
    mat begin = thetaMat;
    double a = moveStep;
    int count = 0;
    while(true)
    {
        e = getDelta(posMat);
        //cout<<norm(e)<<endl;
        if(norm(e) < precision)break;
        jaco = jacobian();
        JtJ = jaco.t() * jacobian();
        a = 0.5 * a;
        dq = inv(JtJ + a * eye(size(JtJ))) * jaco.t() * e;  // caculate the pseudo-inverse matrix of jacobian
        thetaMat += dq; // add the differential move to mat described joint space

        if(count > 100){
            fkine(begin);
            qDebug()<<"超出迭代次数："<<count;
            return begin;
        }
        fkine(thetaMat);
        count++;
    }
    qDebug()<<"迭代次数"<<count;
    if(!checkTheta(thetaMat))
    {
        fkine(begin);
        return begin;
    }
    return thetaMat;
}

/* vex Convert skew-symmetric matrix to vector

  V = vex(S) is the vector which has the corresponding skew-symmetric
  matrix S.

  In the case that S (2x2) =

            | 0  -v |
            | v   0 |

  then V = [v].  In the case that S (3x3) =

            |  0  -vz   vy |
            | vz    0  -vx |
            |-vy   vx    0 |

  then V = [vx; vy; vz].

  Notes::
  - This is the inverse of the function SKEW().
  - Only rudimentary checking (zero diagonal) is done to ensure that the
    matrix is actually skew-symmetric.
  - The function takes the mean of the two elements that correspond to
    each unique element of the matrix.
  - The matrices are the generator matrices for so(2) and so(3).

  References::
  - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p25+43.
 */
mat vex(mat skMetric)
{
    mat v(3,1);
    if(size(skMetric) == SizeMat(3,3))
    {
        v(span(),0) = {{skMetric(2,1)-skMetric(1,2)},{skMetric(0,2)-skMetric(2,0)},{skMetric(1,0)-skMetric(0,1)}};
        v = 0.5 * v;
    }
    else if(size(skMetric)==SizeMat(2,2))
    {
        v(span(),0) = {skMetric(1,0)};
        v = 0.5 * v;
    }
    return v;
}

/* @brief get the differential move of the Decare coordinate in Transmat described by ground coordinate
 *
 */
mat Robot::getDelta(mat endPos) {
    mat delta(6,1);
    mat Td;
    Td = inv(fkine(DH_Mat(span(),3))) * endPos;
    //Td.print();
    delta(span(0,2),0) = Td(span(0,2),3);
    delta(span(3,5),0) = vex(Td(span(0,2),span(0,2)));
    return delta;
}


bool Robot::checkTheta(mat thetaMat)
{
    for(int i = 0;i < Joints.size();i++)
    {
        if(Joints.at(i)->theta_min > thetaMat(i,0) || Joints.at(i)->theta_max < thetaMat(i,0))
        {
            qDebug()<< QString("超过关节%1的限位值%2°！").arg(i+1).arg(thetaMat(i,0)*180/PI);
            return false;
        }
    }
    return true;
}

/* @brief caculate the Transfer mat of DH mat,put the result in the mat T
 *
 */
void Joint::transfer()
{
    double c_t = cos(theta);
    double s_t = sin(theta);
    double c_a = cos(alpha);
    double s_a = sin(alpha);
    this->T = {{c_t,    -s_t*c_a,    s_t*s_a,    a*c_t},
               {s_t,     c_t*c_a,   -c_t*s_a,    a*s_t},
               {  0,         s_a,        c_a,        d},
               {  0,           0,          0,        1}};

}



/* @brief set the value of θ(theta)
 * @param the value of θ
 */
void Joint::set_theta(double value) {
    theta = value;
    transfer();
}

/* @brief set the value of d
 *
 */
void Joint::set_d(double value){
    d = value;
    transfer();
}
